/************************************************************************************************************************************
* Start by putting left hand above shoulder height (pause move) and then switch modes by repeating the same procedure with the right hand.
* When the left hand is lowered, the robot will start to track your right hands movement.
* By spreading your fingers on your left hand you will open the gripper, if you "close" your hand, the gripper will close as well.
* Start recording by waving your left hand about 40 cm in front of you.			//Koi
*************************************************************************************************************************************/
#include <skeleton_listener/Coord.h>
#include <skeleton_listener/Gripper.h>

#include <pthread.h>

#include "ABB_functions_test.h"

CZone zone;
CConnect con;

/*************************************************
* Thread that checks in which zone your hands are
**************************************************/
void *runAll( void *ptr )
{
    bool GripState = true;
    while(ros::ok())
    {
        if(!zone.isPauseZone())
	   break;
        zone.isRecZone();

        if(!zone.mPause && zone.Grip && zone.mGrip!=GripState)
        {
            GripState = zone.mGrip;

            if(zone.mGrip)
                con.writeRead((char*)"<Set Gripper 0>", true);
            else
                con.writeRead((char*)"<Set Gripper 1>", true);
        }

        if(!zone.mPause && zone.Move)
            zone.MoveRobot();
    }

    return 0;
}

/**************************************************
* Callback for updating the positions of the body
***************************************************/
void studSkelCallback(skeleton_listener::Coord c)
{
    zone.mR[0] = c.rx;
    zone.mR[1] = c.ry;
    zone.mR[2] = c.rz;

    zone.mL[0] = c.lx;
    zone.mL[1] = c.ly;
    zone.mL[2] = c.lz;

    zone.mL[3] = c.lsz;
}

/***********************************************
* Callback for hand state, later used by Grip()
************************************************/
void studHandCallback(skeleton_listener::Gripper grip)
{
    zone.mGrip = grip.grip;
}


/********************************
*            Main
*********************************/
int main(int argc, char **argv)
{
    pthread_t threadRun;
    sem_init(&con.critMsg, 0, 1);

    ros::MultiThreadedSpinner spinner(4);

    ros::init(argc, argv, "robot_ROS_interface");
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe("Coord", 10, studSkelCallback);
    ros::Subscriber sub2 = n.subscribe("Gripper", 1, studHandCallback);

    con.connect2Robot(argc,argv);

    zone.init();
    pthread_create( &threadRun, NULL, runAll, NULL);

    spinner.spin();

    ros::waitForShutdown();

    return 0;

}


